#ifndef MATHFUNC_H
#define MATHFUNC_H

#include <Eigen/Dense>
#include <algorithm>
#define SMALL_EPS 1e-30

namespace Estimator
{
using namespace Eigen;

// Convertion from Euler angle to Rotation Matrix
inline Eigen::Matrix3d RotationMatrixFromEulerAngle(const double angles[3])
{
    double alpha = angles[0];
    double beta = angles[1];
    double gamma = angles[2];

    Eigen::Matrix3d Rxalpha;
    Rxalpha << 1, 0, 0,
        0, cos(alpha), -sin(alpha),
        0, sin(alpha), cos(alpha);

    Eigen::Matrix3d Rybeta;
    Rybeta << cos(beta), 0, sin(beta),
        0, 1, 0,
        -sin(beta), 0, cos(beta);

    Eigen::Matrix3d Rzgamma;
    Rzgamma << cos(gamma), -sin(gamma), 0,
        sin(gamma), cos(gamma), 0,
        0, 0, 1;

    return Rzgamma * Rybeta * Rxalpha;
}

// return non-normailized direction in body frame. Rbc is the rotation matrix from camera to body
inline Eigen::Vector3d GetDirectionInBodyFrame(const Eigen::Vector2d& uv, const Eigen::Matrix3d& K, const Eigen::Matrix3d& Rbc = Eigen::Matrix3d::Identity())
{
    Eigen::Vector3d n((uv[0] - K(0, 2)) / K(0, 0), (uv[1] - K(1, 2)) / K(1, 1), 1);
    n.noalias() = Rbc * n;
    return n;
}


inline double GetCosParallax(const Eigen::Matrix3d &R1, const Eigen::Matrix3d &R2,
                    // const Matrix4d &Tbc, 
                    const Eigen::Vector3d &dirCam1, const Eigen::Vector3d &dirCam2, const Eigen::Matrix3d& Rbc)
{
    Eigen::Vector3d n1w =  R1 * Rbc* dirCam1;
    Eigen::Vector3d n2w =  R2 * Rbc* dirCam2;

    return (n1w.dot(n2w)/(n1w.norm()* n2w.norm()));
}

template<class ForwardIt, class T, class Compare>
inline ForwardIt binary_find(ForwardIt first, ForwardIt last, const T& value, Compare comp={})
{
    // Note: BOTH type T and the type after ForwardIt is dereferenced 
    // must be implicitly convertible to BOTH Type1 and Type2, used in Compare. 
    
    first = std::lower_bound(first, last, value, comp);
    return first != last && !comp(value, *first) ? first : last;
}

inline Eigen::Matrix3d skew(const Eigen::Vector3d& v)
{
    Eigen::Matrix3d mat;
    mat(0,0) = 0.0;
    mat(0,1) = -v(2);
    mat(0,2) = v(1);
    mat(1,0) = v(2);
    mat(1,1) = 0.0;
    mat(1,2) = -v(0);
    mat(2,0) = -v(1);
    mat(2,1) = v(0);
    mat(2,2) = 0.0;

    return mat;
}

inline Eigen::Matrix3d dDirection(Eigen::Vector3d &x)
{
    double d = x.norm();
    x.noalias() = x/d; 
    return (Matrix3d::Identity() - x * x.transpose())/d;
}

inline Eigen::Matrix3d Exp(const Eigen::Vector3d& dx)
{
    double theta = dx.norm();
    if (theta < SMALL_EPS)
    {
        return Eigen::Matrix3d::Identity();
    }
    else
    {
        Eigen::Matrix3d hatdx = skew(dx / theta);
        return Eigen::Matrix3d::Identity() + sin(theta) * hatdx + (1 - cos(theta)) * hatdx * hatdx;
    }
}

inline Eigen::Matrix3d Jr(const Eigen::Vector3d &w)
{
    double theta = w.norm();
    if (theta < SMALL_EPS)
    {
        return Eigen::Matrix3d::Identity();
    }
    else
    {
        Eigen::Matrix3d K = skew(w / theta);
        return Eigen::Matrix3d::Identity() - (1 - cos(theta)) / theta * K + (1 - sin(theta) / theta) * K * K;
    }
}

inline Eigen::Matrix3d Jl(const Eigen::Vector3d & w ){
    return Jr(-w);
}


inline void Exp_SE3(Eigen::Matrix3d & dR, Eigen::Matrix3d & Jl, const Eigen::Vector3d & w )
{
    double theta = w.norm();
    if (theta < SMALL_EPS)
    {
        dR =  Eigen::Matrix3d::Identity();
        Jl = dR;
    }
    else
    {
        Eigen::Matrix3d K = skew(w / theta);
        double  st = sin(theta);
        double  ct = cos(theta);
        dR.noalias() = (1 - ct) * K * K;
        dR += Eigen::Matrix3d::Identity() + st * K;
        Jl.noalias() = (1 - st / theta) * K * K;
        Jl += Eigen::Matrix3d::Identity() + (1 - ct) / theta * K;
    }
}


inline  Eigen::Matrix3d RandomRotation(double angle)
{
  double angle1 = ((static_cast<double>(std::rand()) / static_cast<double>(RAND_MAX)) - 0.5) * angle;  

  Eigen::Vector3d rpy;
  rpy[0] = 2.0*((static_cast<double>(std::rand())/ static_cast<double> (RAND_MAX)) - 0.5);
  rpy[1] = 2.0*((static_cast<double>(std::rand())/ static_cast<double> (RAND_MAX)) - 0.5);
  rpy[2] = 2.0*((static_cast<double>(std::rand())/ static_cast<double> (RAND_MAX)) - 0.5);

  rpy.normalize();
  rpy.noalias() = rpy * angle1;
  return Exp(rpy);
}
/*
    All pose are IMU pose in the world frame.
    {feature[0], feature[1]} is {cos(theta), sin(theta)} where theta is the parallax angle between mai anchor pose and associated anchor camera pose.
    {feature[2], feature[3],feature[4]} is the unit vector from the main anchor camera point towards the feature point, expressed in the main anchor camera coordinate frame.
*/
typedef Eigen::Matrix<double, 3, 4, RowMajor> TransformType;
typedef Eigen::Matrix<double, 5, 1> FeatureType;
typedef Eigen::Matrix<double, 3, 12, RowMajor> PoseJacobianType;
typedef Eigen::Matrix<double, 3, 5, RowMajor> FeatureJacobianType;
typedef Eigen::Matrix<double, 3, 6, RowMajor> Matrix36d;



}

#endif // MATHFUNC_H